%**************************************************************************

%Copyright RObotics and Cybernetics Group
%Julian Colorado

%**************************************************************************

%SMA (NiTinol-based) non-linear simulation behavior based on 
%Tanaka's phenomenological models.

%This program computes:
% i) heat transfer model: Temperature Evolution based on power input (current)
% ii) SMA thermo-mechanical model: relationship between stress ? , strain ? and temperature T
% iii) phase transformation kinetics model: Kinematics of a SMA attached to a revolute joint link

clear all

% %Configuration parameters (RIAI paper)
% step =0.1;        %Sampled time
% Time =1;         %Simulation Time
% I =0.3;           %Input current [A].
% 
% To = 20;          %ambient temperature [C]
% m = 0.00014;      %SMA mass [Kg]
% R = 45;           %SMA resistance [Ohms]
% 
% Lo = 0.1;         %link length
% ro = 0.0025;      %Link joint radius
% 
% % Fixed Parameters
% Cp = 0.2;         %Specific heat of wire 
% Ac = 0.0004712;   %SMA wire?s circumferential area per unit length (150um)
% hc = 150;         %Heat convection coefficient

% %Configuration parameters (BaTboT)
% step =0.01;        %Sampled time
% Time =1;         %Simulation Time
% I =0.35;           %Input current [A]. Could be a function.
% 
% 
% %To = 20;          %ambient temperature [C]
% To =[20 22.78 22.73 22.76 22.77 22.82 22.76 22.78 22.76 22.73 22.7 22.74 22.75 22.79 22.8 22.75];
% 
% m = 0.00014;      %SMA mass [Kg]
% R = 8;            %SMA resistance [Ohms]
% 
% Lo = 0.15;      %link length
% ro = 0.005;      %Link joint radius
% 
% % Fixed Parameters
% Cp = 0.2;         %Specific heat of wire 
% Ac = 0.000176;   %SMA wire?s circumferential area per unit length (150um)
% %Ac = 0.0004712;   %SMA wire?s circumferential area per unit length (150um)
% hc = 150;         %Heat convection coefficient



%Configuration parameters (iTuna FISH)
step =0.1;        %Sampled time
Time =1;         %Simulation Time
I =0.35;           %Input current [A]. Could be a function.

To = 20;          %ambient temperature [C]
m = 0.00014;      %SMA mass [Kg]
R = 12;           %SMA resistance [Ohms]

%Lo = 0.175;      %link length
Lo = 0.085;      %link length

ro = 0.0025;      %Link joint radius

% Fixed Parameters
Cp = 0.2;         %Specific heat of wire 
Ac = 0.0004712;   %SMA wire?s circumferential area per unit length (150um)
hc = 150;         %Heat convection coefficient



t = 0:step:Time;  %Time vector
%Initial conditions
T(1) = To(1);        %Initial Temperature [C]
stress(1) = 75;   %Initial stress [MPa]
%strain(1) = 0.01; %Initial strain [MPa]
strain(1) = 0.06; %Initial strain [MPa]
stroke(1) = Lo;
Text = To(1);
%**************************************************************************
%Evolution during Heating
%**************************************************************************
%Temperature [C]
p = length(t);
cont2 = 1;
cont = 1;
tempo =1;

for i=1:p-1
    T(i+1) = step*((I*I*R)-hc*Ac*(T(i)-Text))+T(i);    %Heating Temperature
    cont = cont+1;
    if tempo <= (length(To)-1)
       if cont >  p/(length(To)-1);
            cont2 =cont2+1;
            Text = To(cont2);
            cont = 1;
       end
       tempo = tempo+1;   
    end
end
%Stress computing as a function of temperature
As = T(1);
Af = T(i+1);
aA = pi/(Af-As);
bA = -aA/10.3;     %10.3 is the effect stress constant on Austenite temperatures [MPa.1/C]
p = length(T);
for j=1:p-1
    stress(j+1) = step*(((0.55+1120*(1/(Af-As)))*((T(j+1)-T(j))/step))/(1+1120*(1/(Af-As))))+stress(j);  %Computing stress [MPa]
end

%Martensite fraction computing and its derivative:
p = length(stress);
for k=1:p-1
    M(k) =  0.5*(cos(aA*(T(k)-As)+bA*stress(k))+1);   %Martensite fraction during heating
    dM(k) = -0.5*(sin(aA*(T(k)-As)+bA*stress(k)))*(aA*((T(k+1)-T(k))/step)+bA*((stress(k+1)-stress(k))/step));
    T_h(k) = T(k);
end
Austenite = M;


%Strain computing as a function of stress, temperature, and Marsenite fraction
p = length(M);
for u=1:p-1
    strain(u+1) = (step/75000)*(((stress(u+1)-stress(u))/step)-0.55*((T(u+1)-T(u))/step)+1120*((M(u+1)-M(u))/step))+strain(u);    %Computing strain [MPa]
end


%Kinematics model (SMA attached to a link)
p = length(strain);
Theta(1) = 0;
deltaY = Lo;
for w=1:p-1
    Theta(w+1) = (-step*((Lo*((strain(w+1)-strain(w))/step))/(2*ro)))+Theta(w);
    %***********************
    %for iTuna Fish (forward kinematics, cartesian points)
    Y(w) = -Lo * strain(w)*100 +deltaY;
    X(w) = Y(w)* tan(Theta(w));
    % arch function
    Y_arc(w) = -0.3*X(w)*sin(Theta(w))+0.3*sin(Theta(w));
end
Theta = Theta*(180/pi);
Y = Y*0.1;
% %Resistance as a Function of theta (identified model for Bat robot)
% p = length(Theta);
% for w=1:p
%     Re(w) = -0.08*Theta(w) + 15.05;
% end

%Resistance as a Function of theta (identified model for iTuna-FISH)
Theta2 = Theta.*5;
p = length(Theta);
for w=1:p
    Re(w) = 0.1*Theta2(w) + 8.5;
end



%**************************************************************************
%Evolution during Cooling
%**************************************************************************

%Temperature
i = i+1;
i_flag = i+1;
cont = 1; %flag counter used for knowing how many steps are required in cooling phase
Ms = T(i);
while (T(i) > (To+0.5))
     T(i+1) = step*(-hc*Ac*(T(i)-To))+T(i);      %Cooling temperature
     t(i+1) = t(i)+step;    %Filling time vector with the cooling phase
     i = i+1;
     cont = cont+1;      
end

%Stress computing as a function of temperature
Mf = T(i);
aM = pi/(Ms-Mf);
bM = -aM/10.3;     %10.3 is the effect stress constant on Austenite temperatures [MPa.1/C]


j = j+1;
j_flag = j+1;
for j2=i_flag:cont-1
    stress(j+1) = step*(((0.55+1120*(1/(Ms-Mf)))*((T(j2+1)-T(j2))/step))/(1+1120*(1/(Ms-Mf))))+stress(j);  %Computing stress [MPa]
    j = j+1;
end

%Martensite fraction computing and its derivative:
k = k+1;
j2 = i_flag;
k_flag = k;
temp = 1;
for k2=j_flag:cont-1
    M(k) =  0.5*(cos(aM*(T(j2)-Mf)+bM*stress(k2)))+0.5;   %Martensite fraction during heating
    Martensite(temp) = M(k);
    T_c(temp) = T(j2);
    j2 = j2+1;
    k = k+1;
    temp = temp+1;
end


%Strain computing as a function of stress, temperature, and Marsenite
%fraction
u = u+1;
j2 = i_flag;
k2 = j_flag;

for u2=k_flag:(cont-3)
    strain(u+1) = (step/28000)*(((stress(k2+1)-stress(k2))/step)-0.55*((T(j2+1)-T(j2))/step)+1120*((M(u2+1)-M(u2))/step))+strain(u);    %Computing strain [MPa]
    j2 = j2+1;
    k2 = k2+1;
    u =  u+1;
end

%stroke
ps = length(strain);
for w1=2:ps
    stroke(w1) = Lo-(strain(w1)*Lo);
end

%***************************************************************
% Plotting varibles
%***************************************************************
figure(1), clf
set(gcf,'DoubleBuffer','on')


% Stress VS strain
p1 =length(stress);
p2 =length(strain);
if p1>p2
    p = p2;
else
    p = p1;
end
NN = 1;
%subplot(8,2,15)
subplot(3,2,1)
hold on
grid on;
fig_handles(1+NN) = plot(strain(1:p),stress(1:p),'b');
ylabel('\sigma [MPa]')
xlabel('\epsilon [MPa]')
Title('Stress VS Strain')
set(get(gca,'YLabel'),'Rotation',0.0);


%Temperature VS Time
NN = NN+1;
%subplot(8,2,14)
subplot(3,2,3)
hold on
grid on;
fig_handles(1+NN) = plot(t,T,'b');
ylabel('T [C]')
xlabel('t [s]')
Title('Temperature VS Time')
set(get(gca,'YLabel'),'Rotation',0.0);

% Marteniste Fraction VS Time[s]
p =length(M);
NN = NN+1;
%subplot(8,2,16)
subplot(3,2,5)
hold on
grid on;
fig_handles(1+NN) = plot(T(1:p),stress(1:p),'b');
ylabel('\xi')
xlabel('T [C]')
Title('stress VS Temperature')
set(get(gca,'YLabel'),'Rotation',0.0);
 

% Angle envolution based on strain
p =length(Theta);
NN = NN+1;
%subplot(8,2,16)
subplot(3,2,2)
hold on
grid on;
fig_handles(1+NN) = plot(t(1:p),Theta,'b');
ylabel('\theta [deg]')
xlabel('t [s]')
Title('Angular Position VS Time')
set(get(gca,'YLabel'),'Rotation',0.0);

% Deformation and time
p =length(strain);
NN = NN+1;
%subplot(8,2,16)
subplot(3,2,4)
hold on
grid on;
fig_handles(1+NN) = plot(t(1:ps),stroke(1:ps),'b');
%fig_handles(1+NN) = plot(t(1:p),Lo*ones(p),'b');
ylabel('\epsilon [MPa]')
xlabel('t [s]')
Title('Stroke VS Time')
set(get(gca,'YLabel'),'Rotation',0.0);


% angle estimation based on resistance.
p =length(strain);
NN = NN+1;
%subplot(8,2,16)
subplot(3,2,6)
hold on
grid on;
fig_handles(1+NN) = plot(-Theta2,Re,'b');
%fig_handles(1+NN) = plot(t(1:p),Lo*ones(p),'b');
ylabel('R[Ohms]')
xlabel('\theta [deg]')
Title('Resistance VS Position')
set(get(gca,'YLabel'),'Rotation',0.0);


% %BATBOT
% % Hysteresis plot
% p =length(Austenite);
% for i=1:p
%     Austenite_porc(i) =   (Austenite(i)*100)/1;
% end
% 
% p =length(Martensite);
% for i=1:p
%     Martensite_porc(i) =  (Martensite(i)*100)/1;
% end
% NN = NN+1;
% %subplot(8,2,16)
% subplot(3,2,6)
% hold on
% grid on;
% [AX,H1,H2] = plotyy(T_h,Austenite_porc,T_c,Martensite_porc,'plot');
% set(gca,'XDir','reverse')
% set(gca,'YDir','reverse')
% %ylabel('%Martensite')
% xlabel('T [C]')
% set(get(AX(1),'Ylabel'),'String','% Austenite') 
% set(get(AX(2),'Ylabel'),'String','% Martensite') 
% %set(get(AX(2),'Ylabel'),'FontName','Times New Roman','FontSize',22)
% %set(AX, 'FontSize', 24)
% set(get(AX(2),'Ylabel'),'Rotation',0);
% 
% Title('Hysteresis on SMA')
% set(get(gca,'YLabel'),'Rotation',0.0);

%********************************

% %iTUNA
% % Hysteresis plot
% p =length(Austenite);
% for i=1:p
%     Austenite_porc(i) =   (Austenite(i)*100)/1;
% end
% 
% p =length(Martensite);
% for i=1:p
%     Martensite_porc(i) =  (Martensite(i)*100)/1;
% end
% NN = NN+1;
% %subplot(8,2,16)
% subplot(3,2,6)
% hold on
% grid on;
% [AX,H1,H2] = plotyy(T_h,Austenite_porc,T_c,Martensite_porc,'plot');
% set(gca,'XDir','reverse')
% set(gca,'YDir','reverse')
% %ylabel('%Martensite')
% xlabel('T [C]')
% set(get(AX(1),'Ylabel'),'String','% Austenite') 
% set(get(AX(2),'Ylabel'),'String','% Martensite') 
% %set(get(AX(2),'Ylabel'),'FontName','Times New Roman','FontSize',22)
% %set(AX, 'FontSize', 24)
% set(get(AX(2),'Ylabel'),'Rotation',0);
% 
% Title('Hysteresis on SMA')
% set(get(gca,'YLabel'),'Rotation',0.0);
